#include "cv_canvas.hpp"

#include "zmq_manager.h"

#define HOST_ID 27
#define PUBLISHER_ID SID_NULL
ZmqManager *ZmqMgr = ZmqManager::Create(HOST_ID, PUBLISHER_ID);

#include "rigid2.hpp"

#if 0
void test_rigid2()
{
    Rigid2d trans_one = Rigid2d::Translation(Eigen::Vector2d(2., 3.)) * Rigid2d::Rotation(3.14159265 * 0.5);
    // Rigid2d transd = Rigid2d::Rotation(3.14) * Rigid2d::Translation(Eigen::Vector2d(2., 3.));
    std::cout << "--trans_one----" << std::endl;
    std::cout << trans_one << std::endl;
    std::cout << "------" << std::endl;
    std::cout << trans_one * Eigen::Vector2d(2., 0.) << std::endl;
    std::cout << "--trans_one.inverse()----" << std::endl;
    std::cout << trans_one.inverse() << std::endl;
    std::cout << "------" << std::endl;
    std::cout << trans_one.inverse() * Eigen::Vector2d(2., 0.)  << std::endl;
    //
    Rigid2d trans_two = Rigid2d::Translation(Eigen::Vector2d(3., 5.)) * Rigid2d::Rotation(3.14159265 * 0.5);
    std::cout << "--trans_two----" << std::endl;
    std::cout << trans_two << std::endl;
    std::cout << "--trans_one.inverse() * trans_two----" << std::endl;
    std::cout << trans_one.inverse() * trans_two << std::endl;
    std::cout << "------" << std::endl;
    std::cout << trans_one.inverse() * (trans_two * Eigen::Vector2d(2., 0.)) << std::endl;
    // s
}


void test_pose_case()
{
    cvCanvas::cvCanvasParam param;
    param.minx = -5;
    param.miny = -5;
    cvCanvas canvas(param);
    //
    Pose2D car_pose(2.2f, 3.f, 3.14159f * 0.3333f * 0.5f);
    canvas.AddPoseFrame(car_pose);
    canvas.AddText(car_pose, "car_pose");
    // canvas.SaveToPic("test_cv_canvas1.png");

    // give ultrasound pose in carFrame;
    Pose2D ultra_in_car(0.29f, 0.2f, 3.14159f * 0.25f); // ultra in car 
    Pose2D ultra_pose_global = car_pose + ultra_in_car;
    canvas.AddPoseFrame(ultra_pose_global);
    canvas.AddText(ultra_pose_global, "ultra_pose_global");

    // car corner pos
    std::vector<Pose2D> car_outline;
    car_outline.push_back(Pose2D(0.29f, -0.2f, 0.f));
    car_outline.push_back(Pose2D(0.29f, 0.2f, 0.f));
    car_outline.push_back(Pose2D(-0.2f, 0.2f, 0.f));
    car_outline.push_back(Pose2D(-0.2f, -0.2f, 0.f));

    // canvas.AddPolygon(car_outline, cv::Scalar(2, 0, 222));
    std::vector<Pose2D> car_outline_global;
    for(auto &item:car_outline)
    {
        car_outline_global.push_back(car_pose + item);
    }
    canvas.AddPolygon(car_outline_global, cv::Scalar(2, 0, 222));
    // hit point in ultra
    {
        Pose2D hit_in_ultra(2.f, 0, 0);
        Pose2D hit_in_car = ultra_in_car + hit_in_ultra;
        Pose2D hit_in_global = car_pose + hit_in_car;
        canvas.AddPoint(hit_in_global, cv::Scalar(0, 0, 255), 3);
        canvas.AddLine(ultra_pose_global, hit_in_global, cv::Scalar(222, 0, 255), 1);
    }

    // define motion
    Pose2D motion_move(1.5f, -1.f, -3.14159f * 0.25f);
    {
        Pose2D car_motion_end_pose = car_pose + motion_move;
        {
            // calculate path points
            Pose2D path_point_one = car_pose + Pose2D(0.f, -1.f, 0);
            canvas.AddPoseFrame(path_point_one);
            Pose2D path_point_two = path_point_one + Pose2D(1.5f, 0, 0);
            canvas.AddPoseFrame(path_point_two);
            Pose2D path_point_two_rotated = path_point_two + Pose2D(1.5f, 0, -3.14159f * 0.25f);
            canvas.AddPoseFrame(path_point_two_rotated);
        }
        std::vector<Pose2D> car_outline_global;
        for (auto &item : car_outline)
        {
            car_outline_global.push_back(car_motion_end_pose + item);
        }
        canvas.AddPolygon(car_outline_global, cv::Scalar(2, 0, 222));
        canvas.AddPoseFrame(car_motion_end_pose);
    }

    // pose inverse using ultra_pos_global and car_in_global
    {
        Pose2D ultra_in_car = ultra_pose_global - car_pose;
        canvas.AddPoseFrame(ultra_in_car);
        canvas.AddText(ultra_in_car, "ultra_in_car");
    }
    // 已知机器位姿和一些路径点的位姿，计算路径点相对于机器的位姿
    {
        Pose2D start_pose = Pose2D(2, 4, 0);
        std::vector<Pose2D> path_points;
        path_points.push_back(start_pose);
        float angle_len = 3.24159 * 2;
        float horizon_dis = 6.f;
        float vertical_dis = 2.2f;
        for (float angle = 0; angle < angle_len; angle += (angle_len / 50))
        {
            path_points.push_back(Pose2D(start_pose.X + horizon_dis * angle / angle_len, start_pose.Y + vertical_dis * sin(angle), 0));
        }
        canvas.AddPath(path_points, cv::Scalar(222, 222, 0), 2);
        //
        {
            // calculate points in car 
            std::vector<Pose2D> path_points_in_car;
            for(auto &item:path_points)
            {
                path_points_in_car.push_back(item - car_pose);
            }
            // canvas.AddPath(path_points_in_car, cv::Scalar(222, 222, 0), 2);
            // canvas.AddText(path_points_in_car[0], "path_points_in_car");
            // ratate car
            Pose2D car_rotated = car_pose + Pose2D(0, 0, 3.14159f * 0.5f);
            // calculate points in global after ratated
            {
                std::vector<Pose2D> path_points_rotate_around_car;
                for(auto &item : path_points_in_car)
                {
                    path_points_rotate_around_car.push_back(car_rotated + item);
                }
                canvas.AddPath(path_points_rotate_around_car, cv::Scalar(222, 23, 0), 2);
                canvas.AddText(path_points_rotate_around_car[0], "path_points_rotate_around_car");
            }

        }
    }

    // 
    canvas.SaveToPic("test_cv_canvas1.png");
}


void test_cv_canvas()
{
    cvCanvas::cvCanvasParam param;
    param.minx = -5;
    param.miny = -5;

    cvCanvas canvas(param);
    Pose2D pos(2, 2, 0);
    Pose2D pos2(3, 4, 0);
    canvas.AddPoint(Pose2D(5.4, 5.2, 0));
    canvas.AddLine(pos,pos2);
    {
        // 绘制三角形路径
        std::vector<Pose2D> pathpoints;
        pathpoints.push_back(Pose2D(1.3, 1.3, 0));
        pathpoints.push_back(Pose2D(1.3, 1.6, 0));
        pathpoints.push_back(Pose2D(1.7, 1.3, 0));
        canvas.AddPath(pathpoints, cv::Scalar(222, 0, 0));
    } 
    {
        // 绘制三角形多边形
        std::vector<Pose2D> pathpoints;
        pathpoints.push_back(Pose2D(1.3 + 2, 1.3, 0));
        pathpoints.push_back(Pose2D(1.3 + 2, 1.6, 0));
        pathpoints.push_back(Pose2D(1.7 + 2, 1.3, 0));
        canvas.AddPolygon(pathpoints, cv::Scalar(2, 0, 222));
    }
    {
        // 绘制坐标系
        canvas.AddPoseFrame(Pose2D(6, 6, 3.14159 * 0.3333));
        canvas.AddText(Pose2D(6, 6, 3.24159 * 0.3333), "Frame", cv::Scalar(244, 0, 0));
    }
    {
        // 绘制一个大圆路径点和路径点的切线坐标系
        auto pose = Pose2D(6, 4, 0);
        for(size_t i =0; i< 100; i ++)
        {
            pose = pose + Pose2D(3.24159 *12 / 100, 0, 3.24159 * 2 / 100);
            canvas.AddPoint(pose, cv::Scalar(22, 4, 222), 3);
            canvas.AddPoseFrame(pose);
        }
    }
    {
        // 绘制一个正弦曲线和每个路径点的切线坐标系
        Pose2D start_pose = Pose2D(6, 1, 0);
        std::vector<Pose2D> path_points;
        path_points.push_back(start_pose);
        float angle_len = 3.24159 * 2;
        float horizon_dis = 6.f;
        float vertical_dis = 2.2f;
        for (float angle = 0; angle < angle_len; angle += (angle_len / 50))
        {
            path_points.push_back(Pose2D(start_pose.X + horizon_dis * angle / angle_len, start_pose.Y + vertical_dis * sin(angle), 0));
        }
        canvas.AddPath(path_points, cv::Scalar(222, 222, 0), 2);
        //
        for (size_t i = 0; i < path_points.size() - 1; i++)
        {
            Pose2D frame = path_points[i];
            frame.SetPhi(atan2(path_points[i + 1].Y - path_points[i].Y, path_points[i + 1].X - path_points[i].X));
            canvas.AddPoseFrame(frame, 0.5, 1);
        }
    }
    
    canvas.SaveToPic("test_cv_canvas1.png");
}

#endif
/////////////////// geometry_calculation sample codes /////////////////////////////////////////////////
#include "cv_canvas_new.hpp"
#include "polygon2d.h"

using namespace geometry;

#ifndef DEG2RAD
#define DEG2RAD(x) ((x) * 0.017453292519943)
#endif

Pose2D Vec2dToPose2D(Vec2d vec)
{
    return Pose2D(vec.x(), vec.y(), 0);
}
//
void test_line_segment2d()
{
    // creat canvas for visualization
    cvCanvas::cvCanvasParam param;
    param.minx   = -4;
    param.miny   = -4;
    param.width  = 12;
    param.height = 12;
    // cvCanvas canvas(param);
    CanvasNew canvas(param);
    // 添加一个segment
    LineSegment2d segment({1, 2}, {5, 5});
    canvas.AddLineSegment2d(segment);
    canvas.AddVec2d({1, 2}, CV_RED, 4);
    canvas.AddVec2d({5, 5}, CV_GREEN, 4);
    // 测试 线段绕起点旋转； rotate
    if (1) {
        // 旋转九十度
        Vec2d end_rotated = segment.rotate(DEG2RAD(90)); // 旋转之后的终点
        canvas.AddVec2d(end_rotated, cv::Scalar(0, 220, 0), 4);
        LineSegment2d segment_rotated({segment.start(), end_rotated}); // 旋转之后的线段
        canvas.AddLineSegment2d(segment_rotated);
        canvas.AddText(Vec2dToPose2D(segment.end()), "original segment", CV_BLUE, 1);
        canvas.AddText(Vec2dToPose2D(end_rotated), "segment rotated", CV_BLUE, 1);
    }
    // 测试 计算点到线段的距离； DistanceTo
    if (1) {
        Vec2d p(5, 2);
        canvas.AddVec2d(p, CV_PURPLE, 4);
        printf("--DistanceTo result:%f \n", segment.DistanceTo(p));
    }
    // 测试 计算线段上到外部点最近的点，最多到端点；
    if (0) {
        Vec2d point(5, 2);
        canvas.AddVec2d(point, CV_PURPLE, 4);
        Vec2d nearest_pt;
        segment.DistanceTo(point, &nearest_pt);
        canvas.AddVec2d(nearest_pt, CV_PURPLE, 4);
        // 绘制连线
        canvas.AddLineSegment2d({nearest_pt, point}, CV_BLUE, 2);
        // 将point沿着segment直线方向平移，生成一系列测试点
        if (1) {
            Vec2d unit_dir = segment.unit_direction();
            for (double dis = 0; dis < 5; dis += 0.5) {
                Vec2d pt_translated = point + unit_dir * dis; // 将point沿着segment直线方向平移，生成一系列测试点
                {
                    Vec2d nearest_pt;
                    segment.DistanceTo(pt_translated, &nearest_pt);
                    canvas.AddVec2d(nearest_pt, CV_CYAN, 4);
                    canvas.AddLineSegment2d({nearest_pt, pt_translated}, CV_BLUE, 2);
                }
            }
        }
    }
    // 测试 判断点是否在线段上；
    if (0) {
        Vec2d point    = segment.start();
        Vec2d unit_dir = segment.unit_direction();
        for (double dis = -2; dis < 7; dis += 0.5) {
            Vec2d pt_translated = point + unit_dir * dis; // 将point沿着segment直线方向平移，生成一系列测试点
            {
                // 在线段上的点使用绿色绘制
                if (segment.IsPointIn(pt_translated)) {
                    canvas.AddVec2d(pt_translated, CV_GREEN, 4);
                } else {
                    canvas.AddVec2d(pt_translated, CV_BROWN, 4);
                }
            }
        }
    }
    // 测试 外部点与起点连线在线段方向上的投影距离；
    if (0) {
        Vec2d point(5, 2);
        canvas.AddVec2d(point, CV_PURPLE, 4);
        // 将point沿着segment直线方向平移，生成一系列测试点
        Vec2d unit_dir = segment.unit_direction();
        for (double dis = 0; dis < 4; dis += 0.5) {
            Vec2d pt_translated = point + unit_dir * dis; // 将point沿着segment直线方向平移，生成一系列测试点
            {
                // Vec2d nearest_pt;
                // segment.DistanceTo(pt_translated, &nearest_pt);
                double projected_dis = segment.ProjectOntoUnit(pt_translated);             // 计算投影距离
                canvas.AddVec2d(segment.start() + unit_dir * projected_dis, CV_ORANGE, 4); // 计算投影点
                canvas.AddLineSegment2d({segment.start() + unit_dir * projected_dis, pt_translated}, CV_BLUE, 2);
            }
        }
    }
    // 测试 计算外部点到线段直线的垂直距离；在线段右边的时候距离为负
    if (0) {
        Vec2d point(5, 2);
        Vec2d unit_dir    = segment.unit_direction();
        Vec2d dir_rotated = unit_dir.rotate(DEG2RAD(10)); // unit_dir 顺时针旋转30度
        Vec2d otho_dir    = unit_dir.rotate(DEG2RAD(90)); // 垂直于segment方向
        for (double dis = 0; dis < 4; dis += 0.5) {
            Vec2d pt_translated = point + dir_rotated * dis; // 将point沿着dir_rotated方向平移，生成一系列测试点
            canvas.AddVec2d(pt_translated, CV_BLUE, 4);      // 计算投影点
            {
                // Vec2d nearest_pt;
                // segment.DistanceTo(pt_translated, &nearest_pt);
                double dis     = segment.ProductOntoUnit(pt_translated); // 计算垂直距离
                Vec2d pt_check = pt_translated + otho_dir * abs(dis);    // 将pt_translated沿着垂直于segment方向平移,判断 pt_check 是否在segment上
                canvas.AddVec2d(pt_check, CV_ORANGE, 4);                 // 计算投影点
            }
        }
    }
    // 测试 判断线段是否相交；
    if (0) {
        Vec2d point(5, 2);
        Vec2d other(4, 7);
        canvas.AddVec2d(point, CV_PURPLE, 4);
        // 将point沿着segment直线方向平移，生成一系列测试点
        Vec2d unit_dir = segment.unit_direction();
        for (double dis = 0; dis < 4; dis += 0.5) {
            Vec2d pt_translated = point + unit_dir * dis; // 将point沿着segment直线方向平移，生成一系列测试点
            {
                bool res = segment.HasIntersect({pt_translated, other});
                // 如果相交就用蓝色绘制，不相交就用橙色
                if (res == true) {
                    canvas.AddLineSegment2d({pt_translated, other}, CV_BLUE, 2);
                } else {
                    canvas.AddLineSegment2d({pt_translated, other}, CV_ORANGE, 2);
                }
            }
        }
    }
    // 测试 计算线段的交点；
    if (0) {
        Vec2d point(5, 2);
        Vec2d other(4, 7);
        canvas.AddVec2d(point, CV_PURPLE, 4);
        // 将point沿着segment直线方向平移，生成一系列测试点
        Vec2d unit_dir = segment.unit_direction();
        for (double dis = 0; dis < 4; dis += 0.5) {
            Vec2d pt_translated = point + unit_dir * dis; // 将point沿着segment直线方向平移，生成一系列测试点
            {
                Vec2d intersected_point;
                bool res = segment.GetIntersect({pt_translated, other}, &intersected_point);
                // 如果相交就用蓝色绘制，不相交就用橙色
                if (res == true) {
                    canvas.AddLineSegment2d({pt_translated, other}, CV_BLUE, 2);
                    // 将交点绘制出来
                    canvas.AddVec2d(intersected_point, CV_PURPLE, 4);
                } else {
                    canvas.AddLineSegment2d({pt_translated, other}, CV_ORANGE, 2);
                }
            }
        }
    }
    // 测试 计算点到线段所在直线的投影点；
    if (0) {
        Vec2d point(5, 2);
        canvas.AddVec2d(point, CV_PURPLE, 4);
        // 将point沿着segment直线方向平移，生成一系列测试点
        Vec2d unit_dir = segment.unit_direction();
        for (double dis = 0; dis < 4; dis += 0.5) {
            Vec2d pt_translated = point + unit_dir * dis; // 将point沿着segment直线方向平移，生成一系列测试点
            {
                Vec2d perpendicular_point;
                double dis = segment.GetPerpendicularFoot(pt_translated, &perpendicular_point);
                canvas.AddLineSegment2d({pt_translated, perpendicular_point}, CV_BROWN, 2);
            }
        }
    }

    //
    canvas.SaveToPic("test_line_segment2d.png");
}
int RandomInt(const int s, const int t)
{
    if (s >= t) {
        return s;
    }
    return s + rand() % (t - s + 1);
}
double RandomDouble(double min, double max)
{
    double d = max - min;
    return (((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min;
}

void test_polygon2d()
{
    // creat canvas for visualization
    cvCanvas::cvCanvasParam param;
    param.minx   = -11;
    param.miny   = -11;
    param.width  = 22;
    param.height = 22;
    // cvCanvas canvas(param);
    CanvasNew canvas(param);

    auto fun_print_near = [](double val, double ref, double eps) -> void {
        printf("near [%d] \n", abs(val - ref) < eps);
    };
    auto fun_print_true_false = [](bool res) -> void {
        printf("true/false [%d] \n", res);
    };

    // 测试 点到多边形的距离；点在内部则距离为零；
    if (0) {
        //
        // Polygon2d polygon({{0, 0}, {4, 0}, {4, 2}, {3, 2}, {2, 1}, {1, 2}, {0, 2}});
        Polygon2d polygon(Box2d::CreateAABox({0, 0}, {1, 1}));
        canvas.AddPolygon2d(polygon, CV_BROWN, 2);
        auto func_test_dis = [&](Vec2d pt) -> void {
            double dis = polygon.DistanceTo(pt);
            printf("point[%f,%f] dis to polygon:%f \n", pt.x(), pt.y(), dis);
        };
#if 0
    {
        EXPECT_NEAR(poly1.DistanceTo({0.5, 0.5}), 0.0, 1e-5);

        EXPECT_NEAR(poly1.DistanceTo({-0.2, 0.5}), 0.2, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.2, 0.5}), 0.2, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({0.5, -0.2}), 0.2, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({0.5, 1.2}), 0.2, 1e-5);

        EXPECT_NEAR(poly1.DistanceTo({0, -0.1}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({-0.1, 0}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.0, -0.1}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({-0.1, 1.0}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({0, 1.1}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.1, 0}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.0, 1.1}), 0.1, 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.1, 1.0}), 0.1, 1e-5);

        EXPECT_NEAR(poly1.DistanceTo({-0.1, -0.1}), 0.1 * sqrt(2.0), 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({-0.1, 1.1}), 0.1 * sqrt(2.0), 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.1, -0.1}), 0.1 * sqrt(2.0), 1e-5);
        EXPECT_NEAR(poly1.DistanceTo({1.1, 1.1}), 0.1 * sqrt(2.0), 1e-5);
    }
#endif
        func_test_dis({0.5, 0.5});
        func_test_dis({-0.2, 0.5});
        func_test_dis({1.2, 0.5});
        func_test_dis({0.5, -0.2});
        func_test_dis({0.5, 1.2});
        func_test_dis({0, -0.1});
        func_test_dis({-0.1, 0});
    }
    // 线段到多边形的距离；有点在多边形内部则距离为零；
    if (0) {
        const Polygon2d poly1(Box2d::CreateAABox({0, 0}, {1, 1}));
        canvas.AddPolygon2d(poly1, CV_BROWN, 2);
        {
            fun_print_near(poly1.DistanceTo({{0.5, 0.5}, {1.0, 1.0}}), 0.0, 1e-5);
            fun_print_near(poly1.DistanceTo({{-0.2, 0.5}, {1.2, 0.5}}), 0.0, 1e-5);
            fun_print_near(poly1.DistanceTo({{-2.0, -2.0}, {2.0, 2.0}}), 0.0, 1e-5);
            printf("-------------------\n");
            fun_print_near(poly1.DistanceTo({{-0.2, 0.5}, {-0.2, 0.8}}), 0.2, 1e-5);
            fun_print_near(poly1.DistanceTo({{1.2, 0.5}, {1.2, 0.3}}), 0.2, 1e-5);
            fun_print_near(poly1.DistanceTo({{0.5, -0.2}, {0.8, -0.2}}), 0.2, 1e-5);
            fun_print_near(poly1.DistanceTo({{0.5, 1.2}, {0.3, 1.2}}), 0.2, 1e-5);
            printf("-------------------\n");
            fun_print_near(poly1.DistanceTo({{-0.3, 0.5}, {-0.2, 0.8}}), 0.2, 1e-5);
            fun_print_near(poly1.DistanceTo({{1.2, 0.5}, {1.3, 0.3}}), 0.2, 1e-5);
            fun_print_near(poly1.DistanceTo({{0.5, -0.3}, {0.8, -0.2}}), 0.2, 1e-5);
            fun_print_near(poly1.DistanceTo({{0.5, 1.2}, {0.3, 1.3}}), 0.2, 1e-5);
            printf("-------------------\n");
            fun_print_near(poly1.DistanceTo({{0, -0.1}, {-0.1, 0}}), 0.1 / sqrt(2.0), 1e-5);
            fun_print_near(poly1.DistanceTo({{1.0, 1.1}, {1.1, 1.0}}), 0.1 / sqrt(2.0),
                           1e-5);
            printf("-------------------\n");
            fun_print_near(poly1.DistanceTo({{-10.0, 0.5}, {2.0, 0.5}}), 0.0, 1e-5);
            fun_print_near(poly1.DistanceTo({{-1.0, 0.5}, {10.0, 0.5}}), 0.0, 1e-5);
            printf("-------------------\n");
            fun_print_near(poly1.DistanceTo({{-1.0, 2.0}, {-1.0, 2.0}}), sqrt(2.0), 1e-5);
            fun_print_near(poly1.DistanceTo({{0.5, 0.5}, {0.5, 0.5}}), 0.0, 1e-5);
        }
    }
    // 多边形到多边形的距离；有点在多边形内部则距离为零；
    if (0) {
        const Polygon2d poly1(Box2d::CreateAABox({0, 0}, {1, 1}));
        const Polygon2d poly2({{0, 1}, {1, 0}, {0, -1}, {-1, 0}});
        const Polygon2d poly3(Box2d::CreateAABox({2, 2}, {3, 3}));
        const Polygon2d poly4(Box2d::CreateAABox({-10, -10}, {10, 10}));
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        canvas.AddPolygon2d(poly2, CV_ORANGE, 2);
        canvas.AddPolygon2d(poly3, CV_YELLOW, 2);
        canvas.AddPolygon2d(poly4, CV_GREEN, 2);
        //
        canvas.AddText(Vec2dToPose2D(poly1.GetAllVertices().front()), "poly1");
        canvas.AddText(Vec2dToPose2D(poly2.GetAllVertices().front()), "poly2");
        canvas.AddText(Vec2dToPose2D(poly3.GetAllVertices().front()), "poly3");
        canvas.AddText(Vec2dToPose2D(poly4.GetAllVertices().front()), "poly4");

        fun_print_near(poly1.DistanceTo(poly2), 0.0, 1e-5);
        fun_print_near(poly1.DistanceTo(poly3), sqrt(2.0), 1e-5);
        fun_print_near(poly2.DistanceTo(poly3), 1.5 * sqrt(2.0), 1e-5);
        fun_print_near(poly4.DistanceTo(poly1), 0.0, 1e-5);
        fun_print_near(poly4.DistanceTo(poly2), 0.0, 1e-5);
        fun_print_near(poly4.DistanceTo(poly3), 0.0, 1e-5);
    }
    // 多边形是否在多边形内；多边形是否在多边形内；包含指的是包含所有点都在多边形内部
    if (0) {
        const Polygon2d poly1(Box2d::CreateAABox({0, 0}, {3, 3}));
        const Polygon2d poly2(Box2d::CreateAABox({1, 1}, {2, 2}));
        const Polygon2d poly3(Box2d::CreateAABox({1.5, 1.5}, {4, 4}));
        const Polygon2d poly4(Box2d::CreateAABox({-10, -10}, {10, 10}));
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        canvas.AddPolygon2d(poly2, CV_ORANGE, 2);
        canvas.AddPolygon2d(poly3, CV_YELLOW, 2);
        canvas.AddPolygon2d(poly4, CV_GREEN, 2);
        //
        canvas.AddText(Vec2dToPose2D(poly1.GetAllVertices().back()), "poly1");
        canvas.AddText(Vec2dToPose2D(poly2.GetAllVertices().back()), "poly2");
        canvas.AddText(Vec2dToPose2D(poly3.GetAllVertices().back()), "poly3");
        canvas.AddText(Vec2dToPose2D(poly4.GetAllVertices().back()), "poly4");
        //
        auto func_print_bool = [](bool res) {
            printf("bool is : %d \n", res);
        };
        //
        func_print_bool(poly1.Contains(poly2));
        func_print_bool(poly2.Contains(poly1));

        func_print_bool(poly1.Contains(poly3));
        func_print_bool(poly2.Contains(poly3));
        func_print_bool(poly3.Contains(poly1));
        func_print_bool(poly3.Contains(poly2));

        func_print_bool(poly4.Contains(poly1));
        func_print_bool(poly4.Contains(poly2));
        func_print_bool(poly4.Contains(poly3));
    }

    // 计算两个凸多边形overlap的多边形部分；
    if (0) {
        const Polygon2d poly1(Box2d::CreateAABox({0, 0}, {2, 2}));
        const Polygon2d poly2(Box2d::CreateAABox({1, 1}, {3, 3}));
        const Polygon2d poly3(Box2d::CreateAABox({2, 0}, {4, 2}));
        const Polygon2d poly4(Box2d({2, 2}, M_PI_4, sqrt(2.0), 2));
        //
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        canvas.AddPolygon2d(poly2, CV_ORANGE, 2);
        canvas.AddPolygon2d(poly3, CV_YELLOW, 2);
        canvas.AddPolygon2d(poly4, CV_GREEN, 2);
        //
        canvas.AddText(Vec2dToPose2D(poly1.GetAllVertices().front()), "poly1");
        canvas.AddText(Vec2dToPose2D(poly2.GetAllVertices().front()), "poly2");
        canvas.AddText(Vec2dToPose2D(poly3.GetAllVertices().front()), "poly3");
        canvas.AddText(Vec2dToPose2D(poly4.GetAllVertices().front()), "poly4");
        Polygon2d overlap_polygon;

        fun_print_true_false(poly2.ComputeOverlap(poly4, &overlap_polygon));
        canvas.AddPolygon2d(overlap_polygon, CV_DARKGREY, 1);
    }
    // 计算线段与多边形overlap的部分线段；
    if (0) {
        const Polygon2d poly1(Box2d::CreateAABox({0, 0}, {2, 2}));
        //
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        //
        canvas.AddText(Vec2dToPose2D(poly1.GetAllVertices().front()), "poly1");
        //
        Vec2d first_intersect;
        Vec2d last_intersect;
        fun_print_true_false(poly1.GetOverlap(LineSegment2d({-1, 0}, {-1, 2}),
                                              &first_intersect, &last_intersect));
        fun_print_true_false(poly1.GetOverlap(LineSegment2d({-1, 1}, {-3, 1}),
                                              &first_intersect, &last_intersect));
        fun_print_true_false(poly1.GetOverlap(LineSegment2d({1, 3}, {1, 5}), &first_intersect,
                                              &last_intersect));
        fun_print_true_false(poly1.GetOverlap(LineSegment2d({1, -1}, {2, 3}), &first_intersect,
                                              &last_intersect));
        canvas.AddLineSegment2d({{1, -1}, {2, 3}}, CV_GREEN, 2);
        canvas.AddLineSegment2d({first_intersect, last_intersect}, CV_BLUE, 1);
        fun_print_near(1.0, first_intersect.x(), 1e-5);
        fun_print_near(0.0, first_intersect.y(), 1e-5);
        fun_print_near(1.0, last_intersect.x(), 1e-5);
        fun_print_near(2.0, last_intersect.y(), 1e-5);
    }
    // 计算线段和非凸多边形overlap的那一段或多段，可以ovrelap多段；
    if (0) {
        const Polygon2d poly5(
            {{0, 0}, {4, 0}, {4, 2}, {3, 2}, {2.5, 1}, {2.0, 2}, {2, 1}, {1, 2}, {0, 2}});
        canvas.AddPolygon2d(poly5, CV_RED, 2);
        canvas.AddLineSegment2d({{-10, 1.5}, {6, 1.5}}, CV_GREEN, 2);
        std::vector<LineSegment2d> overlap_line_segments =
            poly5.GetAllOverlaps(LineSegment2d({-10, 1.5}, {6, 1.5}));
        for (auto &item : overlap_line_segments) {
            canvas.AddLineSegment2d(item, CV_BLUE, 1);
        }
    }
    // // 计算点集的凸多边形；
    Polygon2d poly_out;
    if (0) {
        std::vector<Vec2d> points({{0, 0}, {0, 4}, {0, 1}, {0, 3}, {0, 2}, {1, 0}, {1, 1}, {1, 3}, {1, 0}, {3, 0}, {3, 2}, {2, 3}, {2, 0}});
        for (auto &item : points) {
            canvas.AddVec2d(item, CV_PURPLE, 7);
        }
        Polygon2d polygon;
        Polygon2d::ComputeConvexHull(points, &polygon);
        canvas.AddPolygon2d(polygon, CV_CYAN, 2);
        poly_out = polygon;
        // 凸多边形外扩expand; 只能凸多边形往外扩；
        if (1) {
            for (double dis = 0.1; dis < 1; dis += 0.2) {
                Polygon2d exp_poly = poly_out.ExpandByDistance(dis);
                canvas.AddPolygon2d(exp_poly, CV_RED, 2);
            }
        }
    }
    
    // 计算点集的凸多边形；manny points
    if (0) {
        int num = 0;
        for (int iter = 0; iter < 30; ++iter) {
            canvas.Reset();
            const int kRange = 10;
            const int n1     = RandomInt(3, 10);
            const int n2     = RandomInt(3, 10);
            std::vector<Vec2d> points1, points2;
            for (int i = 0; i < n1; ++i) {
                points1.emplace_back(RandomInt(0, kRange), RandomInt(0, kRange));
                canvas.AddVec2d(points1.back(), CV_GREEN, 4);
                printf("[%f,%f]\n", points1.back().x(), points1.back().y());
            }
            for (int i = 0; i < n2; ++i) {
                points2.emplace_back(RandomInt(0, kRange), RandomInt(0, kRange));
                canvas.AddVec2d(points2.back(), CV_RED, 4);
                printf("[%f,%f]\n", points2.back().x(), points2.back().y());
            }

            Polygon2d polygon1;
            Polygon2d polygon2;
            if (!Polygon2d::ComputeConvexHull(points1, &polygon1) ||
                !Polygon2d::ComputeConvexHull(points2, &polygon2)) {
                continue;
            }
            canvas.AddPolygon2d(polygon1, CV_ORANGE, 2);
            canvas.AddPolygon2d(polygon2, CV_MAGENTA, 2);

            Polygon2d overlap_polygon;
            bool res = polygon1.ComputeOverlap(polygon2, &overlap_polygon);
            if (res == true) {
                printf("\n overelap ok\n");
                canvas.AddPolygon2d(overlap_polygon, CV_GREEN, 1);
            } else {
                continue;
            }
            // break;
            num++;
            std::string pic_name = std::string("convexHull_overlap_") + std::to_string(num) + std::string(".png");
            canvas.SaveToPic(pic_name);
        }
    }
    // 计算多边形正的和带heading角度的boundingBox;
    if (0) {
        // Polygon2d poly1(Box2d::CreateAABox({0, 0}, {2, 3}));
        std::vector<Vec2d> points({{0, 0}, {0, 4}, {0, 1}, {0, 3}, {0, 2}, {1, 0}, {1, 1}, {1, 3}, {1, 0}, {3, 0}, {3, 2}, {2, 3}, {2, 0}});
        Polygon2d poly1;
        Polygon2d::ComputeConvexHull(points, &poly1);
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        //
        for (int iter = 0; iter < 30; ++iter) {
            canvas.Reset();
            canvas.AddPolygon2d(poly1, CV_RED, 2);
            Box2d box = poly1.BoundingBoxWithHeading(DEG2RAD(90 / 30 * iter));
            canvas.AddPolygon2d(Polygon2d(box), CV_GREEN_DARK, 1);
            //
            std::string pic_name = std::string("BoundingBoxWithHeading/BoundingBoxWithHeading_") + std::to_string(iter) + std::string(".png");
            canvas.SaveToPic(pic_name);
        }
        Box2d box = poly1.MinAreaBoundingBox();
        canvas.AddPolygon2d(Polygon2d(box), CV_PURPLE, 1);
    }
    // 多边形三角化，多边形分解为多个三角形, 耳切法
    if (1) {
        Polygon2d poly1({{0, 0}, {4, 0}, {4, 2}, {3, 2}, {2.5, 1}, {2.0, 2}, {2, 1}, {1, 2}, {0, 2}});
        // Polygon2d poly1({{0, 0}, {4, 0}, {4, 2}, {0, 4}});
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        std::vector<Polygon2d> triangles_polys = poly1.TriangulationWithEarClipping();
        int num                                = 0;
        for (auto &item : triangles_polys) {
            canvas.AddPolygon2d(item, CV_BLUE, 1);
            canvas.AddText(Vec2dToPose2D(item.GetCentroid()), "tri" + std::to_string(num++));
        }
    }
    //
    canvas.SaveToPic("test_polygon2d.png");
}

// test class box2d
void test_box2d()
{
    // creat canvas for visualization
    cvCanvas::cvCanvasParam param;
    param.minx   = -11;
    param.miny   = -11;
    param.width  = 22;
    param.height = 22;
    CanvasNew canvas(param);
    //
    Box2d box(Box2d::CreateAABox({1, 1}, {3, 4}));
    canvas.AddPolygon2d(Polygon2d(box), CV_RED, 1);

    box.RotateFromCenter(DEG2RAD(45));

    canvas.AddPolygon2d(Polygon2d(box), CV_PURPLE, 1);

    // 判断点是否在box内部 test IsPointIn()
    if (1) {
        for (int iter = 0; iter < 500; ++iter) {
            Vec2d point(RandomDouble(0, 5), RandomDouble(0, 5));
            if (box.IsPointIn(point) == true) {
                canvas.AddVec2d(point, CV_RED, 4);
            } else {
                canvas.AddVec2d(point, CV_BLUE, 4);
            }
        }
    }

    //
    canvas.SaveToPic("test_box2d/test_box2d.png");
}

// 测试多边形凸分解
#include "polygon_partition/out_poly_partition.hpp"
void test_polygon_partition()
{
    cvCanvas::cvCanvasParam param;
    param.minx   = -11;
    param.miny   = -11;
    param.width  = 22;
    param.height = 22;
    CanvasNew canvas(param);
    // 输入一个普通的凹多边形测试
    if (0) {
        Polygon2d poly1({{0, 0}, {4, 0}, {4, 2}, {3, 2}, {2.5, 1}, {2.0, 2}, {2, 1}, {1, 2}, {0, 2}});
        // Polygon2d poly1({{0, 0}, {4, 0}, {4, 2}, {0, 4}});
        canvas.AddPolygon2d(poly1, CV_RED, 2);
        //
        std::vector<Polygon2d> triangles_polys;
        OutPolyPartition::GetPartition(poly1, OutPolyPartition::TRIANGLE_OPT, triangles_polys);
        int num = 0;
        for (auto &item : triangles_polys) {
            canvas.AddPolygon2d(item, CV_BLUE, 1);
            canvas.AddText(Vec2dToPose2D(item.GetCentroid()), "tri" + std::to_string(num++));
        }
    }
    // 从开源项目提供的多边形示例读取多边形
    if (1) {
        Polygon2d polygon_read;
        OutPolyPartition::LoadDefaultTestInputToPolygon2d(polygon_read);
        if (polygon_read.num_points() != 0) {
            printf("load test_input ok; \n");
            //
            canvas.AddPolygon2d(polygon_read, CV_BLUE, 2);
            for (auto &item : polygon_read.points()) {
                canvas.AddVec2d(item, CV_PURPLE, 6);
            }
            canvas.SaveToPic("test_poly_partition/testInput_polygon.png");
        }
        {
            canvas.Reset();
            //
            std::vector<Polygon2d> triangles_polys;
            OutPolyPartition::GetPartition(polygon_read, OutPolyPartition::TRIANGLE_OPT, triangles_polys);
            int num = 0;
            for (auto &item : triangles_polys) {
                canvas.AddPolygon2d(item, CV_RED, 1);
                canvas.AddText(Vec2dToPose2D(item.GetCentroid()), "tri" + std::to_string(num++));
            }
            canvas.SaveToPic("test_poly_partition/test_partition_TRIANGLE_OPT.png");
        }
        {
            canvas.Reset();
            //
            std::vector<Polygon2d> triangles_polys;
            OutPolyPartition::GetPartition(polygon_read, OutPolyPartition::TRIANGLE_MONO, triangles_polys);
            int num = 0;
            for (auto &item : triangles_polys) {
                canvas.AddPolygon2d(item, CV_RED, 1);
                canvas.AddText(Vec2dToPose2D(item.GetCentroid()), "tri" + std::to_string(num++));
            }
            canvas.SaveToPic("test_poly_partition/test_partition_TRIANGLE_MONO.png");
        }
        {
            canvas.Reset();
            //
            std::vector<Polygon2d> triangles_polys;
            OutPolyPartition::GetPartition(polygon_read, OutPolyPartition::CONVEX_OPT, triangles_polys);
            int num = 0;
            for (auto &item : triangles_polys) {
                canvas.AddPolygon2d(item, CV_RED, 1);
                canvas.AddText(Vec2dToPose2D(item.GetCentroid()), "tri" + std::to_string(num++));
            }
            canvas.SaveToPic("test_poly_partition/test_partition_CONVEX_OPT.png");
        }
        {
            canvas.Reset();
            //
            std::vector<Polygon2d> triangles_polys;
            OutPolyPartition::GetPartition(polygon_read, OutPolyPartition::CONVEX_HM, triangles_polys);
            int num = 0;
            for (auto &item : triangles_polys) {
                canvas.AddPolygon2d(item, CV_RED, 1);
                canvas.AddText(Vec2dToPose2D(item.GetCentroid()), "tri" + std::to_string(num++));
            }
            canvas.SaveToPic("test_poly_partition/test_partition_CONVEX_HM.png");
        }
    }
    //
    // canvas.SaveToPic("test_poly_partition/test_polygon_partition.png");
}
#if 0
namespace sss
{

// the function below is wrong!!!
// int RandomInt(const int s, const int t, unsigned int rand_seed)
// {
//     if (s >= t) {
//         return s;
//     }
//     return s + rand_r(&rand_seed) % (t - s + 1);
// }

// this funtion works fine;
int RandomInt(int min, int max)
{
    int d = max - min + 1;
    return int(((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min;
}
void test()
{
    for (int i = 0; i < 100; i++) {
        printf("random num:%d \n", RandomInt(1, 22));
    }
}
}; // namespace sss
#endif

int main()
{
    // sss::test();
    // test_rigid2();
    // test_pose_case();

    // test_line_segment2d();
    // test_polygon2d();
    test_box2d();
    // test_polygon_partition();

    getchar();

    return 0;
}